﻿using System;
using System.Timers;
using Epitech.Krisk.Kinect.Sensor.Tools;
using Microsoft.Research.Kinect.Nui;
using Microsoft.Xna.Framework;

namespace Epitech.Krisk.Kinect.Sensor.Postures
{
    public class PausePosture
    {
        public event EventHandler PauseActivated;
        private DateTime _lastGestureDate = DateTime.Now;
        private Timer _timer = new Timer(2000);

        public PausePosture()
        {
            _timer.AutoReset = true;
            _timer.Elapsed += (sender, args) =>
                                  {
                                      PauseActivated(this, EventArgs.Empty);
                                  };
        }

        private float getAngle(Vector3 v, Vector3 w)
        {
            v.Normalize();
            w.Normalize();
            Vector3 vecteur = Vector3.Cross(v, w);
            float composante = vecteur.X != 0 ? vecteur.X : (vecteur.Y != 0 ? vecteur.Y : vecteur.Z);
            return (float) Math.Acos(Vector3.Dot(v, w))*Math.Sign(composante);
        }

        public void Scan(SkeletonData skeleton)
        {
            Vector3? v1 = null, v2 = null;
            Vector3? shoulder = null, hip = null;
            Vector3? res1 = null, res2 = null;
            foreach (Joint joint in skeleton.Joints)
            {
                switch (joint.ID)
                {
                    case JointID.ShoulderLeft:
                        v1 = joint.Position.ToVector3X();
                        break;
                    case JointID.WristLeft:
                        v2 = joint.Position.ToVector3X();
                        break;
                    case JointID.ShoulderCenter:
                        shoulder = joint.Position.ToVector3X();
                        break;
                    case JointID.HipCenter:
                        hip = joint.Position.ToVector3X();
                        break;
                }
            }
            
            if (v1 != null && v2 != null && shoulder != null && hip != null)
            {
                res1 = v2 - v1;
                res2 = hip - shoulder;

                float angle = Math.Abs(MathHelper.ToDegrees(getAngle(new Vector3(res1.Value.X, res1.Value.Y, 0), new Vector3(res2.Value.X, res2.Value.Y, 0))));
                if (angle < 50 && angle > 40)
                {
                    _timer.Start();

                }
                else
                {
                    _timer.Stop();
                }
            }
        }
    }
}